/**
  ******************************************************************************
  * @file 	zbProtocol.h 
  * @brief  Protocol header file 
  ******************************************************************************
  
ALL LITTLE ENDIAN ! SMALLEST PORTION FIRST !
Name					Size (bytes)	Description
Start of packet / ID	1			0x01 (SOH) default
Transaction Number		1			repeats master
Command					1			See command bytes and their arguments below
Flags					1			Flags to show status of packet, see below definition
Size					2			first size of packet payload 
Payload					Variable	Payload or command arguments
CRC						2			first CRC of entire packet, including header
*/
#include "zbProtocol.h"

//extern u8 g_Vars[g_VarsSize];

// file scope
u16 g_respBytes = 0;
//volatile u32	CRCValue = 0;
//
u8 newPacket[rxMaxPacketSize]; // goes here from circ buffer
// vars we use here
u8 g_PktTXBuff[pktBuffSize];

// timeouts, used mostly in main
u32 pktTimeoutAt = 0x00;
u32 idleTimeoutAt = 0x00;

// file scope rx header vars
//u8 txn = 0;
pktHdr *rxHdr;
pktHdr txHdr;
pktHdr *ptxHdr = &txHdr;
dmaPush dmaTX;
//u16 payloadsize = 0x00;
u32 CRCVal = 0x00;
u32 CRCValCalc = 0x00;

// this cleans out the buffers and sets up pointers
void initProtocol()
{
	u16 i = 0;
	for(i=0; i<rxMaxPacketSize; i++)
	{
		newPacket[i]=0;	
	}
	// at least this will point at something so initial 
	// sleep will not throw hard fault err
	newgVars = (void*)&newPacket[hdrSize+4];
}

//u8 checkPacket(u8 packet[], u16 packetSize)
void checkPacket(CircularBuffer *cb, pktErr *err)
{
	u16 i = 0;
	u16 j = 0;
	u8 found = false;
	//u32 timeoutAt = 0;
	err->SIZE = 0; // how much more do we need 
	// TODO check min size for rw etc so we dont write garbage!

	// if we never got header we need to get that first
	if(err->ERR != 0x01)
	{
		// check to find header before end of packet
		for(i=0; i < cb->count; i++)
		{
			cbRead(cb, &newPacket[0]);
			if(newPacket[0] == ID)
			{
				// copy in rxhdr
				for(j=0; j<=sizeof(rxHdr); j++)
				{
					cbRead(cb, &newPacket[1+j]);	
				}
				
				rxHdr = (void*)&newPacket; 
				found = true;
				// check header 
				if(rxHdr->CMD > 2)
				{
					err->ERR = 4;
					return; 
				}
				break;
			}// end if newpacket

			// we never found anything
			if(cbIsEmpty(cb))
			{
				printf("Packet Info txn: %i size: %i crc: %lX i: %i", rxHdr->TXN, err->SIZE, CRCVal, i);
				err->ERR = 0xFF; // no valid data 
				return; 
			}
		}// end for

	}else{
	   err->ERR = 0x00; // just so i can set breakpoint here
	   found = true; // we found before !
	}
	
	// never found n e thing ! 
	if(found != true)
	{
		 err->ERR = 0xFF;
		 return;
	}

	// do we have all the data available we need ? 
	if(cb->count < (rxHdr->SIZE + crcSize))
	{
		err->ERR = 0x01;
		err->SIZE = (rxHdr->SIZE + crcSize);
		return;
	}

			
	// copy payload into packet 
	for(i = 0; i < rxHdr->SIZE; i++)
	{
	   cbRead(cb, &newPacket[6 + i]);
	}
	// todo check to see if 4 bytes left !
	// what about 4 bytes for crc ?
	cbRead(cb, &newPacket[6 + i]);
	cbRead(cb, &newPacket[7 + i]);
	cbRead(cb, &newPacket[8 + i]);
	cbRead(cb, &newPacket[9 + i]); 
	// TODO if cb != empty something is wrong
	err->ERR = 0x00;	// no error
		

}

//u16 processPacket(u8 packet[], u16 packetSize)
u16 processPacket(void)
{
	u16 respSize = 0, i=0;
	u16 writeSize = 0;
	u16 writeOffset = 0;
	u16 readSize = 0; 
	u16 readoffset = 0;
	u8* packet = newPacket;
	u8* pNewVarArray;
	u8* pPacketPayload;

	// get offset from inc packet
	readoffset = *((u16*)&packet[hdrSize]);
	// get size from inc packet
	readSize = *((u16*)&packet[hdrSize + 2]);
	// setup a array for convience
	pNewVarArray = 	(u8*)newgVars;
	// packet payload starts after header and sizes
	pPacketPayload = packet;

	switch(rxHdr->CMD)
	{
		// read returns x bytes from the glbVars

		case cmd_rw:
			// write values	(note on rw write info starts at +4)
			// get offset from inc packet
			writeOffset = *((u16*)&packet[hdrSize + 4]);
			//writeOffset = writeOffset/4;
			// get size from inc packet
			writeSize = *((u16*)&packet[hdrSize + 6]);
			//writeSize = writeSize/4; // need size in u32 not bytes

			// check to be sure we dont do too much ! 
			if(writeOffset+writeSize > 3096)//g_VarsSize)
			{
				//just return nothing, set error
				respSize = createResponse(g_PktTXBuff, 0x00, 0x01);
				break;
			}
			
			 // note if writesize == 0 nothing happens here ! 
			 // note if its a u32 1=4bytes, u8 1=1byte
			if(writeSize > 0)
			{
				for(i=0; i<writeSize; i+=1)
				{
					pNewVarArray[writeOffset+i] = pPacketPayload[14+i];
				}
			}
			
			// only if we actually had a read size, send response
			if(readSize > 0)
			{
				// setup header 
				txHdr.ID  = rxHdr->ID;
				txHdr.TXN = rxHdr->TXN;
				txHdr.CMD = rxHdr->CMD;
				txHdr.FLG = rxHdr->FLG;
				txHdr.SIZE = readSize; // this is size of payload not whole packet
	
				dmaTX.txHdr = (void*)&txHdr.ID;
				//dmaTX.offset = &g_Vars[*readoffset];
				dmaTX.count = readSize;
				dmaTX.state = new;
				// resp size isnt really used anymore, this should return a bool or error
				respSize = txHdr.SIZE + hdrSize;
			}
			
			break;

		default:
			break; // no response

	}

	// if they set the video flag, go by the video index
	if(rxHdr->FLG & FLAG_VIDEO != 0)
	{
		// if starting on this image get buffered img ready
		// they could be cropping but we expect them to start 
		// within the first horazontal line
		if(readoffset < frame_width)
		// when only two lines left take next pic, so the pic
		// is not so old ! 
		//if(readoffset >= (frame_width * (frame_height/2))*2)
		{
			takeNextSnapShot(); // incraments videostat ! 
		}
		// if they are currently taking pic on A send them B 
		if(mygVars->VideoStat == 1)
		{
	   		dmaTX.offset = &frame_bufferB[readoffset+1];  
	   	}
		// if they are currently taking pic on B send them A 
		if(mygVars->VideoStat == 2)
		{
	   		dmaTX.offset = &frame_bufferA[readoffset+1];  
	   	}
	}else{
		dmaTX.offset = ((u8*) mygVars + readoffset);
	}


	return respSize;
}

// todo make this just return gvars, why copy into another buffer ? 
u16 createResponse(u8 packet[], u16 payloadSize, u8 flags)
{
	int pad,j = 0; // offset to payload

	// put a header on it 
	createHeader(packet, rxHdr->TXN, rxHdr->CMD, flags, payloadSize);
	// append padding
	pad = ((4 - ((hdrSize+payloadSize)%4)) & !4);
	for(j=0; j<pad; j++)
	{
		g_PktTXBuff[hdrSize+payloadSize+j] = pad;
	}
	// append crc
	appendCRC(packet, payloadSize+hdrSize+pad); // crc the payload and header
	// return whole size of packet
	return hdrSize+payloadSize+pad+crcSize;
}

void putPacketInBuffer(u8 payload[], u8 TXN, u8 command, u8 flags, u16 size)
{
//	// counter
//	//u16 i,j,sop;
//	j = 0;
//	//sop = (1024 + hdrSize) * PktBuf; // double buffer
//
//	// stick it in after the header offset
//	for(i = sop + hdrSize; i < size; i++)
//	{
//		//g_PktBuff[i] = payload[j];
//		 j++;
//	}
//
//	// pre pend header, make the size in packet the total size of packet
//	//createHeader(&g_PktBuff[sop], TXN, command, flags, size + hdrSize + 2);
//
//	// append crc, size is all but last 2 bytes, we cant crc the place where crc would be
//	//appendCRC(&g_PktBuff[sop], size + hdrSize);

}

 /**
  * @brief  Creates header at specified location     
  * @note   		
  * @param  location to put header, command of packet, flags
  * @retval bytes in header
  */
u8 createHeader(u8 headerLocation[], u8 TXN, u8 command, u8 flags, u16 payloadSize )
{
	// variables used 
	u8 i;
	
	// fill out vars
	i 		= 0;

	// put in memory as desired
	headerLocation[i++] = 	ID;
	headerLocation[i++] =	TXN;
	headerLocation[i++] =	command;
	headerLocation[i++] =	flags;
	headerLocation[i++] = 	(u8)((payloadSize>>8) & 0xFF);
	headerLocation[i++] =	(u8)(payloadSize & 0xFF);

	return i;
}

 /**
  * @brief  calculates the crc32 for the entire buffer passed in, and puts it just after it    
  * @note   		
  * @param 	databuffer is the packet to get the crc 4, size is the size of said buffer
  * @retval 
  */
void appendCRC(u8 dataBuffer[], u16 sizeBytes)
{
	u32 CRCVal = 0;
	//u16 sizeWords = sizeBytes/4;
	//CRC_ResetDR();
	//CRCVal = CRC_CalcBlockCRC((uint32_t *)dataBuffer, sizeWords);
	u32ToBytes(CRCVal, &dataBuffer[sizeBytes]);
}
